
/**
 * Brandon Yuh 28402055
 * Shang Ling Wu 65226060
 * 
 */
public class MyRobotStrategy extends RobotStrategy {
	private static final int MAX_YELLOW = 2;
	private static final int MAX_RED = 1;
	private int yellowShots = MAX_YELLOW;
	private int redShots = MAX_RED;
	private static final int WIDTH = 6;
	private static final int HEIGHT = 6;
	private boolean initialized = false;
	private static final double PROB_STAY = 0.52;
	private static final double PROB_MOVE = 0.12;
	
	/**
	 * Rename your bot as you please. This name will show up in the GUI.
	 */
	public String getName() { 
		return "HelloBot"; 
	}


	/** 
	 * In this function, we provide you with the sensor information (in the form
	 * of 4 binary values), and the grid coordinates of your robot.
	 * 
	 * The grid is a 6x6 grid, starting at (x=0,y=0) in the top left corner.
	 * We represent this as a flat array "beliefs" with 36 probabilities.
	 * index(x,y) is the index of position (x,y) into the array beliefs  
	 * 
	 * The sensors are in the following order: SWNE, so sensor[1] is West. The
	 * sensors are probabilistic. The vector between your robot and the other
	 * robot is mapped into probabilities of your sensor firing. So if the other
	 * robot is two squares South, and a single square West, then there is a 2/3
	 * chance of the South sensor firing, and a 1/3 chance of the West sensor
	 * firing. Note if the other robot was four squares South and 2 squares
	 * West, your robot's sensors would fire with the same probability.
	 * 
	 * @param sensor
	 *            This round's sensor information
	 * @param xPos
	 *            Robot's X coord
	 * @param yPos
	 *            Robot's Y coord
	 */

	public void updateBeliefState(boolean[] sensor, int xPos, int yPos) {
		if(!initialized){
			//Each robot is placed onto the board uniformly at random, 
			//but at least 4 squares from the opponent
			for (int x = 0; x < WIDTH; x++) {
				for (int y = 0; y < HEIGHT; y++) {
					double dist = Math.abs(x-xPos) + Math.abs(y-yPos);
					if(dist >= 4){
						beliefState[x][y] = 1;
					}
					else{
						beliefState[x][y] = 0;
					}

				}
			}
			initialized = true;
		}
		double[][] transitionProbAndPrior = new double[WIDTH][HEIGHT];
		for (int x = 0; x < WIDTH; x++) {
			for (int y = 0; y < HEIGHT; y++) {
				//Calculate probability Robot stays
				transitionProbAndPrior[x][y] = PROB_STAY;
				
				if(x==0||x==WIDTH-1){
					transitionProbAndPrior[x][y] += PROB_MOVE;
				}
				if(y==0||y==HEIGHT-1){
					transitionProbAndPrior[x][y] += PROB_MOVE;
				}
				transitionProbAndPrior[x][y] *= beliefState[x][y];
				//end stay calculation
				
				//probability robot moves right
				if(x != 0){
					transitionProbAndPrior[x][y] += PROB_MOVE*beliefState[x-1][y];
				}				
				//probability robot moves left
				if(x != WIDTH-1){
					transitionProbAndPrior[x][y] += PROB_MOVE*beliefState[x+1][y];
				}
				//probability robot moves up
				if(y != 0){
					transitionProbAndPrior[x][y] += PROB_MOVE*beliefState[x][y-1];
				}
				//probability robot moves down
				if(y != HEIGHT-1){
					transitionProbAndPrior[x][y] += PROB_MOVE*beliefState[x][y+1];
				}
			}
		}
		double [][] observationProb = new double[WIDTH][HEIGHT];

		for (int x = 0; x < WIDTH; x++) {
			for (int y = 0; y < HEIGHT; y++) {
				observationProb[x][y] = probability(x-xPos,y-yPos,sensor);
			}
		}	
		
		for (int x = 0; x < WIDTH; x++) {
			for (int y = 0; y < HEIGHT; y++) {
				beliefState[x][y] = observationProb[x][y] * transitionProbAndPrior[x][y];
			}
		}

		normalize(beliefState);
	}

	public Order giveOrder(){
		int bestX = 0;
		int bestY = 0;
		double bestValue = 0;
		for (int x = 0; x < WIDTH; x++) {
			for (int y = 0; y < HEIGHT; y++) {
				if (beliefState[x][y] >= bestValue){
					bestValue = beliefState[x][y];
					bestX = x; 
					bestY = y;
				}
			}
		}
		Order o;
		if(bestValue < 0.2)
			o = fireLowProb(bestX, bestY);
		else if (bestValue < 0.3)
			o = fireMedProb(bestX, bestY);
		else
			o = fireHighProb(bestX, bestY);
		return o;
	}
	
	//Try to fire the red cannon, else try to fire the yellow
	private Order fireHighProb(int x, int y){
		if(redShots > 0){
			redShots --;
			return new Order(Order.RED_CANNON, x, y);
		}
		else{
			return fireMedProb(x,y);
		}	
	}
	
	//Try to fire the yellow cannon, else try to fire the green
	private Order fireMedProb(int x, int y) {
		if(yellowShots > 0){
			yellowShots --;
			return new Order(Order.YELLOW_CANNON, x, y);
		}
		else{
			return fireLowProb(x,y);
		}	
	}
	
	//Fire the green cannon
	private Order fireLowProb(int x, int y) {
		return new Order(Order.GREEN_CANNON, x, y);
	}
	
	//Normalize the matrix
	private void normalize(double[][] matrix){
		double sum = 0;
		for (int x = 0; x < matrix.length; x++) {
			for (int y = 0; y < matrix[0].length; y++) {
				sum += matrix[x][y];
			}
		}
		for (int x = 0; x < matrix.length; x++) {
			for (int y = 0; y < matrix[0].length; y++) {
				matrix[x][y]= matrix[x][y]/sum;
			}
		}
	}
	
	//Return the probability given x distance, y distance, the sensors
	private double probability(double x, double y, boolean[] sensor){
		//The sensor is facing the opposite way and says there can't be the robot there
		if(sensor[RobotStrategy.EAST] && x < 0){
			return 0;
		}
		else if(sensor[RobotStrategy.WEST] && x > 0){
			return 0;
		}
		else if (sensor[RobotStrategy.NORTH] && y > 0){
			return 0;
		}
		else if (sensor[RobotStrategy.SOUTH] && y < 0){
			return 0;
		}
		double distance = Math.abs(x) + Math.abs(y);
		if (distance == 0){ 
			return 0;
		}
		double yProb = y / distance;
		double xProb = x / distance;
		
		double prob = 1;
		
		double eProb = xProb;
		if(eProb < 0){
			eProb = 0;
		}
		if (sensor[RobotStrategy.EAST]) {
			prob *= eProb;
		}
		else{
			prob *= 1 - eProb;
		}
		
		double wProb = -xProb;
		if(wProb < 0){
			wProb = 0;
		}
		if (sensor[RobotStrategy.WEST]) {
			prob *= wProb;
		}
		else{
			prob *= 1 - wProb;
		}
		
		double sProb = yProb;
		if(sProb < 0){
			sProb = 0;
		}
		if (sensor[RobotStrategy.SOUTH]){ 
			prob *= sProb;
		}
		else{
			prob *= 1 - sProb;
		}
		
		double nProb = -yProb;
		if(nProb < 0){
			nProb = 0;
		}
		if (sensor[RobotStrategy.NORTH]){ 
			prob *= nProb;
		}
		else{
			prob *= 1 - nProb;
		}
		
		return prob;
	}
}
